﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Threading;
using System.Linq;
using System.Text;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;
using System.Windows.Media.Media3D;
using Microsoft.Kinect;
using System.IO;
//using _3DTools;

namespace trail_WPF_applicaiton
{
    /// <summary>
    /// Interaction logic for MainWindow.xaml
    /// </summary>
    /// 

    public partial class MainWindow : Window
    {

        private KinectSensor sensor;
        private Skeleton[] skeletonData;
        private System.IO.StreamWriter file;
        private bool writeonce= false;
        private bool sim_state = true;
        private bool Draw_state = false; 
        private Vector3D wrist_v_t = new Vector3D(0.0,0.0,0.0);
        private Vector3D wrist_x_t = new Vector3D(0.0, 0.0, 0.0);
        private double K = .03;
        private Skeleton m_skeleton = new Skeleton();
        private Vector3DCollection draw_points = new Vector3DCollection();
        private int count = 0;

        private void WindowLoaded(object sender, RoutedEventArgs e)
        {

            this.file = new System.IO.StreamWriter(@"C:\Users\ashokjal\SkyDrive\kinnect\trail_WPF_applicaiton\trail_WPF_applicaiton\WriteLines2.txt");
            foreach (var potentialSensor in KinectSensor.KinectSensors )
            {
                if (potentialSensor.Status == KinectStatus.Connected)
                {
                    this.sensor = potentialSensor;
                    break;
                }
            }


            if (null != this.sensor)
            {
                this.sensor.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30);
                this.sensor.DepthStream.Enable(DepthImageFormat.Resolution640x480Fps30);
       TransformSmoothParameters smoothingParam = new TransformSmoothParameters();
      {
          smoothingParam.Smoothing = 0.5f;
          smoothingParam.Correction = 0.5f;
          smoothingParam.Prediction = 0.5f;
          smoothingParam.JitterRadius = 0.05f;
          smoothingParam.MaxDeviationRadius = 0.04f;
      };

                this.sensor.SkeletonStream.Enable(smoothingParam);                
                //start the sensor!
                    this.sensor.Start();
            }

            //allocate data for storing Skeleton Frame
            this.skeletonData = new Skeleton[sensor.SkeletonStream.FrameSkeletonArrayLength];

             //   this.kinect_textbox.Text = " and colorStream, DepthStream, SKeleton Stream are working :) ";
            // Try registering an event and calling a function based on the event.
            // this.sensor.SkeletonStream.Enable(TransformSmoothParameters 
            this.sensor.SkeletonFrameReady += sensor_SkeletonFrameReady;
        }


        void sensor_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
        {

            using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame() )
            { 
               if(skeletonFrame != null && this.skeletonData != null)
                {
                    skeletonFrame.CopySkeletonDataTo(this.skeletonData);          
                    this.Log_data.Text = "skeleton Data copied :)";
                }
            }

            foreach (Skeleton skeleton in skeletonData)
            {
                this.m_skeleton = skeleton;
                if (skeleton.TrackingState == SkeletonTrackingState.Tracked & skeleton != null)
                {
                    ClearViewport();
                    Joint wrist_left = skeleton.Joints[JointType.WristLeft];
                    //this.Write_text_file("X : " +wrist_left.Position.Y.ToString());
                    if (this.writeonce)
                    {
                       // this.Record_Positions(skeleton);
                        this.wrist_x_t = this.skeleton_vec3D(wrist_left.Position);
                        this.writeonce = false;
                    
                    }
                    if (this.sim_state)
                    {
                        
                        this.sim_wrist(this.skeleton_vec3D(wrist_left.Position),0.033);
                    }
                    if (Draw_state)
                    {
                        if (count == 10)
                        {
                            //Vector3D new_vec = new Vector3D(this.skeleton_vec3D(wrist_left.Position));
                            this.draw_points.Add(this.skeleton_vec3D(wrist_left.Position));
                            //Enable to draw them;
                            
                            count = 0;
                        }
                        else
                            count += 1;

                        this.RenderDrawings();
                    }
                    this.Log_data.Text = wrist_left.Position.X.ToString();
                    
                   // this.Log_data.Text = "s t :) ";
                    
                    foreach (Joint joint in skeleton.Joints)
                    {
                        Draw_Cube(this.skeleton_vec3D(joint.Position));
                    }
                    /*
                    Draw_Cube(this.skeleton_vec3D(wrist_left.Position));
                    Draw_Cube(this.skeleton_vec3D(skeleton.Joints[JointType.Head].Position));
                    Draw_Cube(this.skeleton_vec3D(skeleton.Joints[JointType.HipCenter].Position));
                    Draw_Cube(this.skeleton_vec3D(skeleton.Joints[JointType.KneeRight].Position));
                    Draw_Cube(this.skeleton_vec3D(skeleton.Joints[JointType.KneeLeft].Position));
                    Draw_Cube(this.skeleton_vec3D(skeleton.Joints[JointType.HipRight].Position));
                    Draw_Cube(this.skeleton_vec3D(skeleton.Joints[JointType.HipLeft].Position));
                    Draw_Cube(this.skeleton_vec3D(skeleton.Joints[JointType.WristRight].Position));
                     */
                   // Draw_Traiangle(this.skeleton_vec3D(skeleton.Joints[JointType.HipCenter].Position));
                   // Draw_Traiangle(this.skeleton_vec3D(skeleton.Joints[JointType.HipCenter].Position));
                   /*
                    foreach (JointType jointType in Enum.GetValues(typeof(JointType)))
                    {
                        Joint joint = skeleton.Joints[jointType];
                    }
                    */

                }
                else if (skeleton.TrackingState == SkeletonTrackingState.PositionOnly)
                {
                    this.Log_data.Text = "s p :) ";
                }


            
            }
            
//            throw new NotImplementedException();
        }


        public void TrackSkeltonLeftHand(object sender, RoutedEventArgs e)
        {
            int count = 0;
            using (SkeletonFrame frame = this.sensor.SkeletonStream.OpenNextFrame(100))
            {
                Skeleton[] data = new Skeleton[frame.SkeletonArrayLength];
                frame.CopySkeletonDataTo(data);

                foreach (Skeleton skeletonData in data)
                {
                    if (skeletonData.TrackingState != SkeletonTrackingState.NotTracked)
                    { 
                       
                        foreach (JointType jointType in Enum.GetValues(typeof(JointType)))
                        {
                             count = count+1;
                            Joint joint = skeletonData.Joints[jointType];
                            this.Log_data.Text = " Came till here. " +count;
                            // Track a log message if joint states are tracked.

                        }
                    }
                
                }

                // Get the position data

            }
        }


        private void WindowClosing(object sender, CancelEventArgs e)
        {
            this.sensor.Stop();
            this.file.Flush();
            this.file.Close();
        }

       
        public MainWindow()
        {
            InitializeComponent();
        }

        void button_Click(object sender, RoutedEventArgs e)
        {
            // Show message box when button is clicked
            this.Close();
        }

        private Vector3D skeleton_vec3D(SkeletonPoint skpt)
        {
            Vector3D vec  = new Vector3D(skpt.X,skpt.Y,skpt.Z);
            this.Log_data.Text = vec.ToString() ;
            return vec;
        
        }

        private void Draw_Traiangle(Vector3D vec)
        {
            //vec = vec ;

            MeshGeometry3D triangleMesh = new MeshGeometry3D();
            Point3D point0 = new Point3D(vec.X, vec.Y, vec.Z);
            Point3D point1 = new Point3D(vec.X + .4, vec.Y, vec.Z);
            Point3D point2 = new Point3D(vec.X, vec.Y, vec.Z + .4 );
            triangleMesh.Positions.Add(point0);
            triangleMesh.Positions.Add(point1);
            triangleMesh.Positions.Add(point2);
            triangleMesh.TriangleIndices.Add(0);
            triangleMesh.TriangleIndices.Add(2);
            triangleMesh.TriangleIndices.Add(1);
            Vector3D normal = new Vector3D(0, 1, 0);
            triangleMesh.Normals.Add(normal);
            triangleMesh.Normals.Add(normal);
            triangleMesh.Normals.Add(normal);
            Material material = new DiffuseMaterial(
                new SolidColorBrush(Colors.Red));
            GeometryModel3D triangleModel = new GeometryModel3D(
                triangleMesh, material);
            ModelVisual3D model = new ModelVisual3D();
            model.Content = triangleModel;

            this.mainViewport.Children.Add(model); 
        }

        private Model3DGroup CreateTriangleModel(Point3D p0, Point3D p1, Point3D p2)
        {
            MeshGeometry3D mesh = new MeshGeometry3D();
            mesh.Positions.Add(p0);
            mesh.Positions.Add(p1);
            mesh.Positions.Add(p2);
            mesh.TriangleIndices.Add(0);
            mesh.TriangleIndices.Add(1);
            mesh.TriangleIndices.Add(2);
            Vector3D normal = CalculateNormal(p0, p1, p2);
            mesh.Normals.Add(normal);
            mesh.Normals.Add(normal);
            mesh.Normals.Add(normal);
            Material material = new DiffuseMaterial(
                new SolidColorBrush(Colors.DarkKhaki));
            GeometryModel3D model = new GeometryModel3D(
                mesh, material);
            Model3DGroup group = new Model3DGroup();
            group.Children.Add(model);
            return group;
        }

        private Vector3D CalculateNormal(Point3D p0, Point3D p1, Point3D p2)
        {
            Vector3D v0 = new Vector3D(
                p1.X - p0.X, p1.Y - p0.Y, p1.Z - p0.Z);
            Vector3D v1 = new Vector3D(
                p2.X - p1.X, p2.Y - p1.Y, p2.Z - p1.Z);
            return Vector3D.CrossProduct(v0, v1);
        }

        private void Draw_Cube(Vector3D vec)
        {
            Model3DGroup cube = new Model3DGroup();
            float delta = 0.05f;
            Point3D p0 = new Point3D(vec.X, vec.Y, vec.Z);
            Point3D p1 = new Point3D(vec.X + delta, vec.Y, vec.Z);
            Point3D p2 = new Point3D(vec.X + delta, vec.Y, vec.Z+delta);
            Point3D p3 = new Point3D(vec.X, vec.Y, vec.Z+delta);
            Point3D p4 = new Point3D(vec.X, vec.Y+delta, vec.Z);
            Point3D p5 = new Point3D(vec.X+delta, vec.Y+delta, vec.Z);
            Point3D p6 = new Point3D(vec.X+delta, vec.Y+delta, vec.Z+delta);
            Point3D p7 = new Point3D(vec.X, vec.Y+delta, vec.Z+delta);
            //front side triangles
            cube.Children.Add(CreateTriangleModel(p3, p2, p6));
            cube.Children.Add(CreateTriangleModel(p3, p6, p7));
            //right side triangles
            cube.Children.Add(CreateTriangleModel(p2, p1, p5));
            cube.Children.Add(CreateTriangleModel(p2, p5, p6));
            //back side triangles
            cube.Children.Add(CreateTriangleModel(p1, p0, p4));
            cube.Children.Add(CreateTriangleModel(p1, p4, p5));
            //left side triangles
            cube.Children.Add(CreateTriangleModel(p0, p3, p7));
            cube.Children.Add(CreateTriangleModel(p0, p7, p4));
            //top side triangles
            cube.Children.Add(CreateTriangleModel(p7, p6, p5));
            cube.Children.Add(CreateTriangleModel(p7, p5, p4));
            //bottom side triangles
            cube.Children.Add(CreateTriangleModel(p2, p3, p0));
            cube.Children.Add(CreateTriangleModel(p2, p0, p1));

            ModelVisual3D model = new ModelVisual3D();
            model.Content = cube;
            this.mainViewport.Children.Add(model);
        }

        private void ClearViewport()
        {
            ModelVisual3D m;
            for (int i = this.mainViewport.Children.Count - 1; i >= 0; i--)
            {
                m = (ModelVisual3D)mainViewport.Children[i];
                if (m.Content is DirectionalLight == false)
                    this.mainViewport.Children.Remove(m);
            }
        }

        private void simpleButtonClick(object sender, RoutedEventArgs e)
        {
            //Toggle near mode
            if (this.simpleButton.Content == "Near Mode")
            {
                this.sensor.SkeletonStream.EnableTrackingInNearRange = true;
                this.simpleButton.Content = "Far Mode";
            }
            else
            {
                this.sensor.SkeletonStream.EnableTrackingInNearRange = false;
                this.simpleButton.Content = "Near Mode";
            }
             
        }


        private void SetCamera()
        {
//            cam_pos_x = 0 + 2 * cos(teta);
  //          cam_pos_z = 0 + 2 * sin(teta);
            PerspectiveCamera camera = (PerspectiveCamera)mainViewport.Camera;
            Point3D position = new Point3D(
                Convert.ToDouble(cameraPositionXTextBox.Text),
                Convert.ToDouble(cameraPositionYTextBox.Text),
                Convert.ToDouble(cameraPositionZTextBox.Text)
            );
            Vector3D lookDirection = new Vector3D(
                Convert.ToDouble(lookAtXTextBox.Text),
                Convert.ToDouble(lookAtYTextBox.Text),
                Convert.ToDouble(lookAtZTextBox.Text)
            );
            camera.Position = position;
            camera.LookDirection = lookDirection;
        }

        private void SetCamera_hipJoint()
        {

        }

        private void Write_text_file(string str) 
        {
            this.file.WriteLine(str);
        }

        private void Record_Positions(Skeleton skeleton)
        {
            this.file.WriteLine("Head");
            this.file.WriteLine(this.skeleton_vec3D(skeleton.Joints[JointType.Head].Position));
            this.file.WriteLine("Hip");
            this.file.WriteLine(this.skeleton_vec3D(skeleton.Joints[JointType.HipCenter].Position));
            this.file.WriteLine("Left_wrist");
            this.file.WriteLine(this.skeleton_vec3D(skeleton.Joints[JointType.WristLeft].Position));
            this.file.WriteLine("Right_Wrist");
            this.file.WriteLine(this.skeleton_vec3D(skeleton.Joints[JointType.WristRight].Position));
            this.file.WriteLine("Left_knee");
            this.file.WriteLine(this.skeleton_vec3D(skeleton.Joints[JointType.KneeLeft].Position));
            this.file.WriteLine("Right_knee");
            this.file.WriteLine(this.skeleton_vec3D(skeleton.Joints[JointType.KneeRight].Position));
            this.file.WriteLine("Left_foot");
            this.file.WriteLine(this.skeleton_vec3D(skeleton.Joints[JointType.FootLeft].Position));
            this.file.WriteLine("Right_foot");
            this.file.WriteLine(this.skeleton_vec3D(skeleton.Joints[JointType.FootRight].Position));
        }

        private void sim_wrist(Vector3D wrist_vec,double delta_t)
        {
            Vector3D delta_X = -this.wrist_x_t+wrist_vec;
            //explicit Integration
            /*
            this.wrist_v_t += delta_t * K * delta_X;
            delta_X += this.wrist_v_t * delta_t;
            // implicit Integration
            */
            this.wrist_v_t = (this.wrist_v_t + (delta_t * this.K * this.wrist_x_t)) / (1 + this.K * delta_t * delta_t);
            delta_X += delta_t * this.wrist_v_t;
            
            
            this.wrist_x_t = -delta_X +wrist_vec ;
            //Draw_Traiangle(new Vector3D(0, 0, 2));
           // Draw_Traiangle(wrist_vec);
            Draw_Traiangle(this.wrist_x_t );//+ new Vector3D(0.2, 0.2, 0.2));
        }


        private void SetCameraButton(object sender, RoutedEventArgs e)
        {
            SetCamera();
        }

        private void Record_position_button_Click_1(object sender, RoutedEventArgs e)
        {
            this.writeonce = true;
        }

        private void Sim_Start_Click_1(object sender, RoutedEventArgs e)
        {
            if (this.sim_state)
                this.sim_state = false;
            else
                this.sim_state = true;

           // this.wrist_x_t = this.skeleton_vec3D(wrist_left.Position);
        }

        private void Draw_Click(object sender, RoutedEventArgs e)
        {
            //do nothing as of now.
            if (this.Draw_state)
                this.Draw_state = false;
            else
                this.Draw_state = true;
        }

        private void RenderDrawings()
        {
            foreach (Vector3D vec in this.draw_points)
            {
                Draw_Cube(vec);
            }
        
        }

        /*
        private void RenderLines()
        {
            ScreenSpaceLines3D wireframe = new ScreenSpaceLines3D();
            foreach (Vector3D vect in this.draw_points) ;
            {
                Point3D point = new Point3D(vect.X, vect.Y, vect.Z);
            }
            wireframe.Points.Add();
            wireframe.Points.Add(p0);
            wireframe.Points.Add(p1);
            wireframe.Points.Add(p2);
            wireframe.Points.Add(p0);
            wireframe.Color = Colors.LightBlue;
            wireframe.Thickness = 3;
            this.mainViewport.Children.Add(wireframe);

        }*/


    
    }
}
